/*
Blurry Savant preliminary library

Author : Pedro Bittencourt
Date : 28/11/2009 - 29/11/2009
Last Review in: 02/12/2009 by: Pedro Bittencourt
*/

#include <stdio.h>
#include <math.h>
#include <stdlib.h>

#include "proto_controller.h"

/* 
Out is a structure to represent a fuzzy output 
of a Mamdani inference process
*/
typedef struct out{
  int group;
  double cut;
  struct out *next;
}Out;

Out* createOutPut(){
  return ((Out*)malloc(sizeof(Out)));
}

/* A sample defuzzyfication method (Mean of Maxima) */
int MOMDefuzz(Out* out){
  Out* t = out;
  int mom = 0,k = 0;
  double maxCut = 0;
  while(t){
    maxCut = MAX(maxCut,t->cut);
    t = t->next;
  }
  t = out;
  while(t){
    if(t->cut == maxCut){
      mom+=t->group;
      k++;
    }
    t = t->next;
  }
  return ((int) round(mom/k));
}

void printMatrix(int* mat,int x,int y){
  int i=0,j=0;
  for(i=0;i<y;i++){
    for(j=0;j<x;j++){
      printf("%d ",mat[i+y*j]);
    }
    printf("\n");
  }
}

/* A degree of pertinence evaluation function */
double checkPert(int input,int group){
  int diff = abs(group-input);
  switch(diff){
    case 0: return 1.0;
    case 1: return 0.7;
    case 2: return 0.3;
    default: return 0.0;
  }
}

/* Inference function */
int fuzzyInf(double err,double derr, double gx, double gy, int* rules,int ( * defuzzy ) ( Out * )){
  int xe,ye;
  int i = 0, j = 0, k = 0;
  double m1,m2;
  Out *outPut,*current;
  
  outPut = createOutPut();
  current = outPut;
  
  xe = QUANT(gx,err);
  ye = QUANT(gy,derr);
  
  for(i=0;i<NUMOFGROUPS;i++){
    for(j=0;j<NUMOFGROUPS;j++){'
      m1 = checkPert(xe,2*i-6);
      m2 = checkPert(ye,2*j-6);
      if(m1!=0 && m2!=0){
        k++;
        current->group = rules[i+NUMOFGROUPS*j];
        current->cut = MIN(m1,m2);
        current->next = createOutPut();
        current = current->next;
      }
    }
  }
  current = NULL;
  return defuzzy(outPut);
  
}
/* Main for preliminary test purposes,it's not part of the project */
int main(){
  double g1,g2;
  int* rules;
  int i = 0, j = 0;
  int* outTable;
  Out* outPut;
  Out* current;
  outPut = createOutPut();
  current = outPut;
  rules = ((int*)malloc(NUMOFGROUPS*NUMOFGROUPS*sizeof(int)));
  outTable = ((int*)malloc(NUMOFRULES*sizeof(int)));
  for(i=0;i<NUMOFGROUPS;i++){
    for(j=0;j<NUMOFGROUPS;j++){
      if((2*i-6)<0){
        if((2*j-6)<-4) rules[i+NUMOFGROUPS*j] = 6;
        else if((2*j-6)<0) rules[i+NUMOFGROUPS*j] = 4;
        else if((2*j-6)>=0) rules[i+NUMOFGROUPS*j] = -2;
      }
      if((2*i-6)>=0){
        if((2*j-6)<-4) rules[i+NUMOFGROUPS*j] = 4;
        else if((2*j-6)<=0) rules[i+NUMOFGROUPS*j] = 2;
        else if((2*j-6)>0) rules[i+NUMOFGROUPS*j] = -4;      
      }
    }
  }
  printMatrix(rules,NUMOFGROUPS,NUMOFGROUPS);
  printf("enter scale factors for x and y:\n");
  scanf("%lf",&g1);
  scanf("%lf",&g2);
  for(i=0;i<ALLVALUES;i++){
    for(j=0;j<ALLVALUES;j++){
      outTable[i+ALLVALUES*j] = fuzzyInf(i-6,j-6,g1,g2, rules,MOMDefuzz);
    }
  }
  printMatrix(outTable,ALLVALUES,ALLVALUES);
  return 0;
}
